﻿using BZ.Core;
using HalconDotNet;
using Lmi3d.GoSdk;
using Lmi3d.GoSdk.Messages;
using Lmi3d.Zen;
using Lmi3d.Zen.Io;
using System;
using System.Collections.Concurrent;
using System.Runtime.ExceptionServices;
using System.Threading;

namespace NDK.AcqDevice
{
    public class GocatorController : ILineLaser
    {
        private HImage heightImage;
        private HImage luminanceImage;
        private AutoResetEvent heightImageEvent = new AutoResetEvent(false);
        private AutoResetEvent luminanceImageEvent = new AutoResetEvent(false);
        public ConcurrentQueue<HImage> CQueneHeightImage { get; set; }

        private GoSystem system;
        private GoSensor sensor;
        private KIpAddress ipAddress;
        private GoDataSet dataSet;

        public GocatorController() { }

        public HImage HeightImage(int timeout = 3000)
        {
            if (heightImageEvent.WaitOne(timeout) == true)
            {
                if (heightImage.IsInitialized())
                {
                    return heightImage.CopyImage();
                }
            }

            return null;
        }
        public HImage LuminanceImage(int timeout = 3000)
        {
            if (luminanceImageEvent.WaitOne(timeout) == true)
            {
                if (luminanceImage.IsInitialized())
                {
                    return luminanceImage.CopyImage();
                }
            }
            return null;
        }
        public bool IsConnected
        {
            get
            {
                if (sensor == null)
                {
                    return false;
                }
                return sensor.IsConnected();
            }
        }
        /// <summary>
        /// 连接传感器并初始化，可以通过IP或则SN连接
        /// </summary>
        /// <param name="id">传感器的唯一标识：IP或则SN，推荐用SN</param>
        /// <returns></returns>
        public bool Connect(string id, int portID)
        {
            try
            {
                KApiLib.Construct();
                GoSdkLib.Construct();
                system = new GoSystem();
                if (id.Contains("."))
                {
                    ipAddress = KIpAddress.Parse(id);
                    sensor = system.FindSensorByIpAddress(ipAddress);
                }
                else
                {
                    uint sn = uint.Parse(id);
                    sensor = system.FindSensorById(sn);
                }
                sensor.Connect();

                //GoSetup setup = sensor.Setup;
                sensor.EnableData(true);
                sensor.SetDataHandler(onData);
                system.DataCapacity = 524288000;
                CQueneHeightImage = new ConcurrentQueue<HImage>();
                return true;

            }
            catch (Exception ex)
            {
                throw new LaserException("Gocator Connect Exception! StackTrace:" + ex.StackTrace);
            }
        }
        /// <summary>
        /// 触发，开始采集
        /// </summary>
        /// <returns></returns>
        public bool Start()
        {
            if (sensor == null)
            {
                return false;
            }
            try
            {
                heightImageEvent.Reset();
                luminanceImageEvent.Reset();
                if (sensor.State != GoState.Ready)
                {
                    sensor.Stop();
                    Thread.Sleep(100);
                }
                sensor.Start();
                CQueneHeightImage = new ConcurrentQueue<HImage>();
            }
            catch (Exception ex)
            {
                throw new LaserException("Gocator system.Start Exception! StackTrace:" + ex.StackTrace);
            }
            return true;
        }

        /// <summary>
        /// 停止触发
        /// </summary>
        /// <returns></returns>
        public bool Stop()
        {
            if (sensor == null)
            {
                return false;
            }
            try
            {
                sensor.Stop();
                CQueneHeightImage = new ConcurrentQueue<HImage>();
            }
            catch (Exception ex)
            {
                throw new LaserException("Gocator system.Stop Exception! StackTrace:" + ex.StackTrace);
            }
            return true;
        }
        /// <summary>
        /// 停止采集高度图，当长时间没有高度图像返回时，可调用此方法结束等待
        /// </summary>
        /// <returns></returns>
        public bool StopAcqHeightImage()
        {
            heightImageEvent.Set();
            return Stop();
        }
        /// <summary>
        /// 停止采集亮度图，当长时间没有亮度图像返回时，可调用此方法结束等待
        /// </summary>
        /// <returns></returns>
        public bool StopAcqLuminanceImage()
        {
            luminanceImageEvent.Set();
            return Stop();
        }
        /// <summary>
        /// 加载Job
        /// </summary>
        /// <param name="name">Job名称</param>
        /// <returns></returns>
        public bool SwitchJob(string name)
        {
            if (sensor == null)
            {
                return false;
            }
            try
            {
                sensor.CopyFile(name + ".job", "_live.job");
                sensor.DefaultJob = name + ".job";
            }
            catch (Exception ex)
            {
                throw new LaserException("Gocator SwitchDefaultJobName Exception! StackTrace:" + ex.StackTrace);
            }
            return true;
        }

        public void LMIChangeScanLength(double length)
        {
            try
            {
                if (sensor.State != GoState.Ready)
                    Thread.Sleep(100);
                GoSetup setup = sensor.Setup;
                GoSurfaceGeneration goSurfaceGeneration = setup.GetSurfaceGeneration();
                //goSurfaceGeneration.FixedLengthStartTrigger = 1; //启用外部触发1
                goSurfaceGeneration.FixedLengthLength = length;
                sensor.Flush();
                //sensor.CopyFile("_live.job", "Product.job");
            }
            catch (Exception)
            {
                Stop();
                Thread.Sleep(100);
                GoSetup setup = sensor.Setup;
                GoSurfaceGeneration goSurfaceGeneration = setup.GetSurfaceGeneration();
                //goSurfaceGeneration.FixedLengthStartTrigger = 1; //启用外部触发1
                goSurfaceGeneration.FixedLengthLength = length;
                sensor.Flush();
                //sensor.CopyFile("_live.job", "Product.job");
            }
            //Thread.Sleep(80);
        }
        public bool ClearData()
        {
            if (system == null)
            {
                return false;
            }
            try
            {
                system.ClearData();
            }
            catch (Exception ex)
            {
                throw new LaserException("Gocator ClearData Exception! StackTrace:" + ex.StackTrace);
            }
            return true;
        }
        /// <summary>
        /// 点云采集回调函数
        /// </summary>
        /// <param name="data"></param>
        [HandleProcessCorruptedStateExceptions]
        /// 
        private void onData(KObject data)
        {
            dataSet = (GoDataSet)data;
            for (uint i = 0; i < dataSet.Count; i++)
            {
                GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
                switch (dataObj.MessageType)
                {
                    case GoDataMessageType.UniformSurface:
                        {
                            try
                            {
                                GoUniformSurfaceMsg goSurfaceMsg = (GoUniformSurfaceMsg)dataObj; // 定义变量gosurfacemsg,类型gosurfacemsg
                                long length = goSurfaceMsg.Length;    //surface长度
                                long width = goSurfaceMsg.Width;      //surface宽度
                                long bufferSize = width * length;
                                double XResolution = goSurfaceMsg.XResolution / 1000000.0;  //surface 数据X方向分辨率为nm,转为mm
                                double YResolution = goSurfaceMsg.YResolution / 1000000.0;  //surface 数据Y方向分辨率为nm,转为mm
                                double ZResolution = goSurfaceMsg.ZResolution / 1000000.0;  //surface 数据X方向分辨率为nm,转为mm
                                //double XOffset = goSurfaceMsg.XOffset / 1000.0;             //接收到surface数据X方向补偿单位um，转mm
                                //double YOffset = goSurfaceMsg.YOffset / 1000.0;             //接收到surface数据Y方向补偿单位um，转mm
                                double ZOffset = goSurfaceMsg.ZOffset / 1000.0;             //接收到surface数据Z方向补偿单位um，转mm
                                                                                            //long rowldx, colldx;
                                IntPtr bufferPointer = goSurfaceMsg.Data;

                                heightImage = new HImage();
                                heightImage.GenImage1("int2", (int)width, (int)length, bufferPointer);
                                heightImage = heightImage.ConvertImageType("real");
                                heightImage = heightImage.ScaleImage(ZResolution, ZOffset);
                                CQueneHeightImage.Enqueue(heightImage);
                            }
                            catch (Exception ex)
                            {
                                throw new LaserException("Gocator UniformSurface Exception! StackTrace:" + ex.StackTrace);
                            }
                            finally
                            {
                                heightImageEvent.Set();
                            }
                        }
                        break;
                    case GoDataMessageType.SurfaceIntensity://亮度图
                        {
                            try
                            {
                                GoSurfaceIntensityMsg surfaceMsg = (GoSurfaceIntensityMsg)dataObj;
                                long width = surfaceMsg.Width;
                                long height = surfaceMsg.Length;
                                long bufferSize = width * height;
                                double XResolution = surfaceMsg.XResolution / 1000000.0;  //surface 数据X方向分辨率为nm,转为mm
                                double YResolution = surfaceMsg.YResolution / 1000000.0;  //surface 数据Y方向分辨率为nm,转为mm
                                double XOffset = surfaceMsg.XOffset / 1000.0;             //接收到surface数据X方向补偿单位um，转mm
                                double YOffset = surfaceMsg.YOffset / 1000.0;             //接收到surface数据Y方向补偿单位um，转mm
                                IntPtr bufferPointeri = surfaceMsg.Data;

                                luminanceImage = new HImage();
                                luminanceImage.GenImage1("byte", (int)width, (int)height, bufferPointeri);

                            }
                            catch (Exception ex)
                            {
                                throw new LaserException("Gocator SurfaceIntensity Exception! StackTrace:" + ex.StackTrace);
                            }
                            finally
                            {
                                luminanceImageEvent.Set();
                            }
                        }
                        break;
                    default:
                        break;
                }
            }
        }


        public bool SDKToHalconImage(HObject Image, HTuple YResolution, HTuple XResolution, HTuple ZResolution, HTuple ZOffset, out HImage outImage)
        {
            try
            {
                HTuple hv_HomMat2DIdentity = new HTuple(), hv_HomMat2DScale = new HTuple();
                HObject ho_Image1, ho_ImageScaled;
                HObject ho_ImageScaled1, ho_ImageConverted;
                hv_HomMat2DIdentity.Dispose();
                HOperatorSet.HomMat2dIdentity(out hv_HomMat2DIdentity);
                using (HDevDisposeHelper dh = new HDevDisposeHelper())
                {
                    hv_HomMat2DScale.Dispose();
                    if (YResolution > XResolution)
                    {
                        HOperatorSet.HomMat2dScale(hv_HomMat2DIdentity, 1, YResolution / XResolution, 0, 0, out hv_HomMat2DScale);
                    }
                    else
                    {
                        HOperatorSet.HomMat2dScale(hv_HomMat2DIdentity, YResolution / XResolution, 1, 0, 0, out hv_HomMat2DScale);
                    }
                }
                HOperatorSet.AffineTransImage(Image, out ho_Image1, hv_HomMat2DScale, "constant", "true");

                using (HDevDisposeHelper dh = new HDevDisposeHelper())
                {
                    HOperatorSet.ScaleImage(ho_Image1, out ho_ImageScaled, 1, -ZOffset);
                }
                using (HDevDisposeHelper dh = new HDevDisposeHelper())
                {
                    HOperatorSet.ScaleImage(ho_ImageScaled, out ho_ImageScaled1, 1 / ZResolution, 32768);
                }
                HOperatorSet.ConvertImageType(ho_ImageScaled1, out ho_ImageConverted, "uint2");
                outImage = new HImage(ho_ImageConverted);
            }
            catch (Exception)
            {
                outImage = new HImage();
            }

            return true;
        }

        public void DisConnect()
        {
            try
            {
                sensor.Disconnect();
            }
            catch (Exception)
            {

            }
        }
    }
}
